
module StrainSensorNewNewP {

	provides {
		interface SerialSensorRead as Read;
		interface Msp430UartConfigure;
		interface Init ;
	}
	uses {



	  interface GeneralIO  as Power ;
	  interface UartStream as	SensorStream;
	  interface	Resource;


	  interface Timer<TMilli> as CommandInterleavingTimer ;
	  interface Timer<TMilli> as AckTimeoutTimer ;
	  interface Timer<TMilli> as SensorReadingTimer ;

	  interface	Leds;

	}

}
implementation {

	msp430_uart_union_config_t msp430_uart_telos_config = {
		{ ubr: UBR_1MHZ_9600,
			umctl: UMCTL_1MHZ_9600,
			ssel: 0x02,
			pena: 0,
			pev: 0,
			spb: 0,
			clen: 1,
			listen: 0,
			mm: 0,
			ckpl: 0,
			urxse: 0,
			urxeie: 1,
			urxwie: 0,
			utxe : 1,
			urxe : 1}
	};


	enum{

		COMMAND_INTERLEAVING = 512U,//256U, //256,//2048, //2048,//512,
		COMMAND_BOOT_TIME = 5120U,
		ACK_TIMEOUT = 512U,				// If I do not receive an ack, I assume the sensor has not received the command
		SENSOR_READING_TIMEOUT = 8192U, // This is the time to wait when a new channel request is sent (it takes 5+2 seconds max).

		CMD_REQUEST_SAMPLE = 0x1,
		CMD_BOOT = 0x0,

		  PARSER_BEGIN_ACK_1 = 0,
		  PARSER_BEGIN_ACK_2 = 1,

		  PARSER_ACK_1 = 2,
		  PARSER_ACK_2 = 3,

		  PARSER_END_ACK_1 = 4,
		  PARSER_END_ACK_2 = 5,

		  PARSER_BEGIN_VALUE_1 = 6,
		  PARSER_BEGIN_VALUE_2 = 7,

		  PARSER_CHANNEL_VALUE_1 = 8,
		  PARSER_CHANNEL_VALUE_2 = 9,
		  PARSER_TIME_VALUE_1 = 10,
		  PARSER_TIME_VALUE_2 = 11,
		  PARSER_TIME_VALUE_3 = 12,
		  PARSER_TIME_VALUE_4 = 13,
		  PARSER_TIME_VALUE_5 = 14,
		  PARSER_TEMP_VALUE_1 = 15,
		  PARSER_TEMP_VALUE_2 = 16,
		  PARSER_TEMP_VALUE_3 = 17,

		  PARSER_END_VALUE_1 = 18,
		  PARSER_END_VALUE_2 = 19
	};

	serial_sensor_data_t sensor_data ;


	uint8_t ack_channel = 0 ;
	uint8_t value_channel = 0 ;
	uint8_t requested_channel = 0 ;

	bool m_busy = FALSE;

	task void read();

	task void request_resource() ;
	task void request_sample() ;
	task void send_single_channel_request() ;

	static void parse(uint8_t) ;
	static bool isDigit(uint8_t) ;
	static bool isNewLine(uint8_t) ;
	static bool isDot(uint8_t) ;
	static void loadValues() ;
	static void storeValues() ;


	task void sendCommand() ;

	task void switchChannel() ;

	task void stopAckTimeoutTimer() ;
	task void startAckTimeoutTimer() ;
	task void stopSensorReadingTimer() ;
	task void startSensorReadingTimer() ;




  // These values are set to zero at each single channel request.

  uint8_t m_cmd ;

  uint32_t start ;
  uint32_t stop ;

  uint8_t cmd[6] ;

  uint16_t parser_temperature = 0 ;
  uint16_t parser_time = 0 ;
  uint16_t parser_channel = 0 ;

  bool channel_mismatch = FALSE ;

  uint8_t parser_state ;

  bool not_ready_detected = FALSE ;

  command error_t Init.init(){
	  call Power.makeOutput(); // this command should be placed at init or boot
	  return SUCCESS ;
  }

	command error_t Read.read() {
		if ( m_busy == TRUE ){
//			printf("%s, EBUSY\n", __FUNCTION__);
//			printfflush();
			return EBUSY;
		}

		start = call CommandInterleavingTimer.getNow() ;
		m_busy = TRUE;
		atomic{
			requested_channel = SENSOR_COUNT ;
		}
		post read();

		return SUCCESS;
	}

task void request_resource(){
	call Resource.request();
}

task void readDone(){
//	uint8_t i ;
	call Leds.set( 0 );
	call Power.clr();
	call Resource.release() ; // never knows...

	stop = call CommandInterleavingTimer.getNow() ;

	atomic{
		sensor_data.sensor_timer = stop - start ;
	}

//	for(i = 0; i<SENSOR_COUNT; i++){
//		atomic{
//		printf("channel %u - freq: %u , temp: %u\n",i+1,sensor_data.sensor[i].data, sensor_data.sensor[i].temp ) ;
//		}
//	}
//	printfflush() ;

	m_busy = FALSE ;
	signal Read.readDone(SUCCESS,&sensor_data) ;
}

	task void read(){

		uint8_t i ;

		for(i=0; i<6; i++){
atomic{
			sensor_data.sensor[i].data = 0xffff ;
			sensor_data.sensor[i].temp = 0xffff ;
}
		}

		// Power on the board
		call Leds.set( 7 );
		call Power.set();


		// By default it starts with the first channel
//		printf("Start...\n") ;
//		printfflush() ;
		m_cmd = CMD_BOOT ;
		call CommandInterleavingTimer.startOneShot(COMMAND_BOOT_TIME) ;
	}

	event void CommandInterleavingTimer.fired(){
		switch(m_cmd){
			case CMD_BOOT:
				post sendCommand() ;
			break;
			case CMD_REQUEST_SAMPLE:
				post request_sample() ;
			break;
		}
	}

	// is fired after an ack if no sensor data comes within SENSOR_READING_TIMEOUT
	// or it is fired when a not ready is read and no sensor_data comes within SENSOR_READING_TIMEOUT
	// what to do: store the timeout error in the temp value, store values, release the resource and switch to next channel.
	event void SensorReadingTimer.fired(){
//		printf("Sensor reading timeout...\n") ;
//		printfflush() ;
		atomic{
			parser_temperature = 0xfffe ;
		}
		storeValues() ;
		call Resource.release() ;
		post switchChannel() ;
	}

	// is fired when the node send a command and the sensor does not reply
	// what to do: store the timeout error in the temp value, store values, release the resource and switch to next channel.
	event void AckTimeoutTimer.fired(){
//		printf("Ack timeout...\n") ;
//		printfflush() ;
		atomic{
			parser_temperature = 0xfffd ;
		}
		storeValues() ;
		call Resource.release() ;
		post switchChannel() ;
	}

	task void request_sample(){
		// reset values
		atomic{
			parser_state = PARSER_BEGIN_ACK_1 ;
		}

//		printf("Request resource\n") ;
//		printfflush() ;

		// Ask the resource to send a command.
		if (call Resource.immediateRequest() != SUCCESS ) {
			post request_resource() ; // if it hangs here (i.e., the resource is never granted), the timeout releases the resource and send a new command.
			return ;
		}
		post send_single_channel_request() ;
	}

	task void switchChannel(){
		// switch to next channel
		if(requested_channel > 1){
			atomic{
				requested_channel-- ;
			}

			// starts the procedure for the next channel
			post sendCommand() ;
		}
		else{
			post readDone() ;
		}

	}


	task void send_single_channel_request(){
		// reset parser field since we expect new values.
		atomic{
		parser_temperature = 0 ;
		parser_time = 0 ;
		ack_channel = 0 ;
		value_channel = 0 ;
		channel_mismatch = FALSE ;
		not_ready_detected = FALSE ;
		}
		cmd[0] = ':';
		cmd[1] = 'G' ; //requested_channel + '0' ;
		cmd[2] = 'C' ; //13 ;
		cmd[3] = '?';
		cmd[4] = requested_channel+'0';
		cmd[5] = 13 ;
//		printf("Sending command\n") ;
//		printfflush() ;

		if(call SensorStream.send( cmd, 6) != SUCCESS){
			post send_single_channel_request() ;
		}
	}

static void storeValues(){
	atomic{
	sensor_data.sensor[requested_channel-1].data = parser_time ;
	sensor_data.sensor[requested_channel-1].temp = parser_temperature ;
	}
}


static bool isDigit(uint8_t value){
	if(value >= 48 && value <= 57) return TRUE ;
	else return FALSE ;
}

static bool isChannelDigit(uint8_t value){
	if(value >= 49 && value <= 54) return TRUE ;
	else return FALSE ;
}

static bool isNewLine(uint8_t value){
	return (value == 10)? TRUE:FALSE ;
}

static bool isDot(uint8_t value){
	return (value == 46)? TRUE:FALSE ;
}

static uint8_t toInt(uint8_t value){
	return value - '0' ;
}

static bool isCR(uint8_t value){
	return (value == 13)? TRUE:FALSE;
}

static bool isAck(uint8_t value){
	return (value == 65)? TRUE:FALSE;
}

static bool isNotReady(uint8_t value){
	return (value == 78)? TRUE:FALSE;
}

static bool isNoStrain(uint8_t value){
	return (value == 88)? TRUE:FALSE;
}

static bool isTimeout(uint8_t value){
	return (value == 84)? TRUE:FALSE;
}

static void parse(uint8_t value){
	switch(parser_state){

	case PARSER_BEGIN_ACK_1:
		if(isCR(value)){ // recognized string: \r
			parser_state = PARSER_BEGIN_ACK_2;
		}
		else{
			parser_state = PARSER_BEGIN_ACK_1;
			// error, expecting cr begin ack
		}
		break;


	case PARSER_BEGIN_ACK_2:
		if(isNewLine(value)){ // recognized string: \r\n
			parser_state = PARSER_ACK_1;
		}
		else{
			parser_state = PARSER_BEGIN_ACK_1;
			//error, expecting new line begin ack.
		}
		break;

	case PARSER_ACK_1:
		if(isAck(value)){ // recognized string: \r\nA
			parser_state = PARSER_ACK_2;
		}
		else{
			parser_state = PARSER_BEGIN_ACK_1;
			// error, expecting ack.
		}
		break;

	case PARSER_ACK_2:
		if(isChannelDigit(value)){ // recognized string: \r\nAc where c is a char value in [1-6]
			parser_state = PARSER_END_ACK_1;
			ack_channel = toInt(value) ;
			if(requested_channel != ack_channel){
				// error channel mismatch!
				parser_time = 0xfffb ;
				parser_temperature = ack_channel ;
				channel_mismatch = TRUE ;
			}
		}
		else{
			parser_state = PARSER_BEGIN_ACK_1;
			// error, expecting digit during ack
		}
		break;

	case PARSER_END_ACK_1:
		if(isNewLine(value)){ // recognized string: \r\nAc\n
			parser_state = PARSER_END_ACK_2;
		}
		else{
			parser_state = PARSER_BEGIN_ACK_1;
			// error, expecting new line
		}
		break;

	case PARSER_END_ACK_2:
		if(isNewLine(value)){ // recognized string: \r\nAc\n\n -> OK, now should wait some values
			parser_state = PARSER_BEGIN_VALUE_1 ;
			post stopAckTimeoutTimer() ;
//			printf("Ack!\n") ;
//			printfflush() ;
			post startSensorReadingTimer() ;
		}
		else{
			parser_state = PARSER_BEGIN_ACK_1;
			// error, expecting new line
		}
		break;

	case PARSER_BEGIN_VALUE_1:
		if(isCR(value)){ // recognized string: \r
			parser_state = PARSER_BEGIN_VALUE_2 ;
		}
		else{
			parser_state = PARSER_BEGIN_ACK_1;
			// error, expecting CR during begin value
		}
		break;

	case PARSER_BEGIN_VALUE_2:
		if(isNewLine(value)){ // recognized string: \r\n
			parser_state = PARSER_CHANNEL_VALUE_1 ;
		}
		else{
			parser_state = PARSER_BEGIN_ACK_1;
			// error, expecting new line
		}
		break;

	case PARSER_CHANNEL_VALUE_1:
		if(isChannelDigit(value)){ // recognized string: \r\nC where C is a char value in [1-6]
			parser_state = PARSER_CHANNEL_VALUE_2 ;
			value_channel = toInt(value) ;
			if(requested_channel != value_channel){
				// error channel mismatch!
				parser_time = 0xfffc ;
				parser_temperature = value_channel ;
				channel_mismatch = TRUE ;
			}
		}
		else if(isNotReady(value)){ // recognized string: \r\nN -> need to wait for a new value (without ack)
			not_ready_detected = TRUE ;
			value_channel = requested_channel ;
			parser_state = PARSER_END_VALUE_1 ;
		}
		else if(isNoStrain(value)){ // recognized string: \r\nX -> finished, set an error value and go to the end of the parser state to switch the channel
			parser_state = PARSER_END_VALUE_1 ;
			value_channel = requested_channel ;
			parser_time = 0xfffd ;
		}
		else if(isTimeout(value)){ // recognized string: \r\nT -> finished, set an error value and go to the end of the parser state to switch the channel
			parser_state = PARSER_END_VALUE_1 ;
			value_channel = requested_channel ;
			parser_time = 0xfffe ;
		}
		else{
			parser_state = PARSER_BEGIN_ACK_1;
			// error, unexpected state
		}
		break;

	case PARSER_CHANNEL_VALUE_2:
		if(isNewLine(value)){ // recognized string: \r\nC\n
			parser_state = PARSER_TIME_VALUE_1;
		}
		else{
			parser_state = PARSER_BEGIN_ACK_1;
			// error, expecting new line at parser value
		}
		break;

	case PARSER_TIME_VALUE_1:
		if(isDigit(value)){ // recognized string: \r\nC\nK where K is a value in [0-9]
			parser_state = PARSER_TIME_VALUE_2;
			if(!channel_mismatch)parser_time = toInt(value) ;
		}
		else{
			parser_state = PARSER_BEGIN_ACK_1;
		}
		break;

	case PARSER_TIME_VALUE_2:
		if(isDigit(value)){ // recognized string: \r\nC\nKK where K is a value in [0-9]
			parser_state = PARSER_TIME_VALUE_3;
			if(!channel_mismatch)parser_time = parser_time*10 + toInt(value) ;
		}
		else{
			parser_state = PARSER_BEGIN_ACK_1;
		}
		break;

	case PARSER_TIME_VALUE_3:
		if(isDigit(value)){ // recognized string: \r\nC\nKKK where K is a value in [0-9]
			parser_state = PARSER_TIME_VALUE_4;
			if(!channel_mismatch)parser_time = parser_time*10 + toInt(value) ;
		}
		else{
			parser_state = PARSER_BEGIN_ACK_1;
		}
		break;

	case PARSER_TIME_VALUE_4:
		if(isDigit(value)){ // recognized string: \r\nC\nKKKK where K is a value in [0-9]
			parser_state = PARSER_TIME_VALUE_5;
			if(!channel_mismatch)parser_time = parser_time*10 + toInt(value);
		}
		else{
			parser_state = PARSER_BEGIN_ACK_1;
		}
		break;

	case PARSER_TIME_VALUE_5:
		if(isNewLine(value)){ // recognized string: \r\nC\nKKKK\n
			parser_state = PARSER_TEMP_VALUE_1;
		}
		else{
			parser_state = PARSER_BEGIN_ACK_1;
		}
		break;

	case PARSER_TEMP_VALUE_1:
		if(isDigit(value)){ // recognized string: \r\nC\nKKKK\nM where M is a value in [0-9]
			parser_state = PARSER_TEMP_VALUE_2;
			if(!channel_mismatch)parser_temperature = toInt(value) ;
		}
		else{
			parser_state = PARSER_BEGIN_ACK_1;
		}
		break;

	case PARSER_TEMP_VALUE_2:
		if(isDigit(value)){ // recognized string: \r\nC\nKKKK\nMM where M is a value in [0-9]
			parser_state = PARSER_TEMP_VALUE_3;
			if(!channel_mismatch)parser_temperature = parser_temperature*10 + toInt(value) ;
		}
		else{
			parser_state = PARSER_BEGIN_ACK_1;
		}
		break;

	case PARSER_TEMP_VALUE_3:
		if(isDigit(value)){ // recognized string: \r\nC\nKKKK\nMMM where M is a value in [0-9]
			parser_state = PARSER_END_VALUE_1;
			if(!channel_mismatch)parser_temperature = parser_temperature*10 + toInt(value) ;
		}
		else{
			parser_state = PARSER_BEGIN_ACK_1;
		}
		break;

	case PARSER_END_VALUE_1:
		if(isNewLine(value)){ // recognized string: \r\nC\nKKKK\nMMM\n OR \r\nX\n OR \r\nT\n OR \r\nN\n
			parser_state = PARSER_END_VALUE_2;
		}
		else{
			parser_state = PARSER_BEGIN_ACK_1;
		}
		break;

	case PARSER_END_VALUE_2:
		if(isNewLine(value)){ // recognized string: \r\nC\nKKKK\nMMM\n\n OR \r\nX\n\n OR \r\nT\n\n OR \r\nN\n\n
			if(not_ready_detected){
				parser_state = PARSER_BEGIN_VALUE_1 ; // after a not ready, wait for a value without ack.
				not_ready_detected = FALSE ;
				post startSensorReadingTimer() ;

			}
			else{
				parser_state = PARSER_BEGIN_ACK_1;

				post stopSensorReadingTimer() ;

				storeValues() ;

				call Resource.release() ;
				post switchChannel() ;
			}
		}
		else{
			parser_state = PARSER_BEGIN_ACK_1;
		}
		break;

	}
}

task void stopAckTimeoutTimer(){
	if(call AckTimeoutTimer.isRunning()){
		call AckTimeoutTimer.stop() ;
	}
}

task void startAckTimeoutTimer(){
	call AckTimeoutTimer.startOneShot(ACK_TIMEOUT) ;
}

task void stopSensorReadingTimer(){
	if(call SensorReadingTimer.isRunning()){
		call SensorReadingTimer.stop() ;
	}
}


task void startSensorReadingTimer(){
	call SensorReadingTimer.startOneShot(SENSOR_READING_TIMEOUT) ;
}

	async event void SensorStream.receivedByte( uint8_t data ) {

//		printf("_%u",data) ;
//		printfflush() ;

		parse(data) ;

	}

	task void sendCommand(){
		m_cmd = CMD_REQUEST_SAMPLE ;
		call CommandInterleavingTimer.startOneShot(COMMAND_INTERLEAVING) ;
	}

	async event void SensorStream.sendDone( uint8_t* buf, uint16_t len, error_t error )
	{
		post startAckTimeoutTimer() ;
	}

	async event void SensorStream.receiveDone( uint8_t* buf, uint16_t len, error_t error ) {}

	async command msp430_uart_union_config_t* Msp430UartConfigure.getConfig() {
		return & msp430_uart_telos_config;
	}

	event void Resource.granted() {
//		printf("Granted\n") ;
//		printfflush() ;
		// now we have the uart ready, we can send the command
		post send_single_channel_request() ;

	}

}
